#include <stdio.h>
#include "VEXRobot.h"
#include <adc.h>
#include "ifi_utilities.h"
#include <UserAPI.h>
#include "serial_ports.h"

static int  adcPort = 1;
static int  intPort = 1;
static int  intMask = 1;
static unsigned long MasterTicCountStart;

enum ADStep
{
   ADSTEP_First,
   ADSTEP_Second
};

static enum ADStep adcStep = ADSTEP_First;
enum USStep
{
   USSTEP_Start,
   USSTEP_Wait,
   USSTEP_Value,
   USSTEP_Stop
};
static enum USStep  ultrasonicStep = USSTEP_Start;

void MSRSInitialize()
{
   // Initialize the COM ports
   Init_Serial_Port_One(BAUD_115200);
   Init_Serial_Port_Two(BAUD_9600);

   // Reset the ultrasonic ports
   {
      int   i;
      for(i=0; i<NUM_INTERUPTS; ++i)
      {
         ultrasonicIOPort[i] = 0;
      }
   }

   // Start a free-running timer....
   // I'll preset it when it's needed
#ifdef TIMER
   StartTimer(1);
#endif
}

static short cnt = 0;
void Process_Data_From_Local_IO(void)
{
   // Try to receive and process a command from the PC
   ProcessCommand();

   // Ok, now some housekeeping.
   // Process the ADC ports
   if(adcPort <= numAdc)
   {
      // Read the ADC port twice and use the second reading
      // This allows the port to switch to the port and settle
      switch(adcStep)
      {
      case ADSTEP_First:
         adcValues[adcPort-1] = Get_Analog_Value( adcPort );
         adcStep = ADSTEP_Second;
         break;
      case ADSTEP_Second:
         adcValues[adcPort-1] = Get_Analog_Value( adcPort );
         adcStep = ADSTEP_First;
         ++adcPort;
         break;
      }

   } else {
      // Process the interrupts
      if(intPort <= NUM_INTERUPTS)
      {
         // See if the interrupt is used by the ultrasonic sensor
         if(sonarMask & intMask)
         {
            switch(ultrasonicStep)
            {
            case USSTEP_Start:
               MyStartUltrasonic(intPort, ultrasonicIOPort[intPort-1]);
               ultrasonicStep = USSTEP_Wait;
               MasterTicCountStart = MasterTicCount + 10L;
               break;

            case USSTEP_Wait:
               // Read the first value and save it.
               // Experience has shown that the device is very 'bouncy'
               // Read it twice and take the lower value.
               if(MyUltrasonicReady())
               {
                  ultrasonicStep = USSTEP_Value;
               }
               else
               {
                  if(MasterTicCountStart < MasterTicCount)
                  {
                     interruptValues[intPort-1] = -1;
                     ultrasonicStep = USSTEP_Stop;
                  }
               }
               break;
            case USSTEP_Value:
               interruptValues[intPort-1] = MyGetUltrasonic();
               ultrasonicStep = USSTEP_Stop;
               break;

            case USSTEP_Stop:
               MyStopUltrasonic();
               // Take the lower reading
               ultrasonicStep = USSTEP_Start;
               ++intPort;
               intMask <<= 1;
               break;
            }
         }

         // Read the encoder
         else if(encoderMask & intMask)
         {
            interruptValues[intPort-1] = GetEncoder(intPort);
            ++intPort;
         }

         // Not used, so reset to 0
         else
         {
            interruptValues[intPort-1] = 0;
            ++intPort;
         }
      }
      // Reset to the first device again (ADC)
      else {
         adcPort = 1;
         intPort = 1;
         intMask = 1;
         ultrasonicStep = USSTEP_Start;
         adcStep = ADSTEP_First;
      }
   }
}
